#include <common.h>
#include <oem_func.h>
#define readl(addr) (*(volatile unsigned int*)(addr)) 
#define writel(b,addr) ((*(volatile unsigned int *) (addr)) = (b))

#define DISK_PARTITIONED	0x359FC953
#ifdef CONFIG_SYS_DISK_USB
#include <usb.h>
static int oem_sys_disk_get_devnum(void)
{
	struct usb_device *dev = NULL;
	int i=0;
	for(;i<4;){
		dev = usb_get_dev_index(i);
		if(dev->portnr==CONFIG_SYS_DISK_CHANNEL) break;
		i++;
	}
	return dev->devnum;
}
static int oem_sys_disk_get_desc(block_dev_desc_t *dev_desc,int retry, int* index)
{
	block_dev_desc_t *dev=NULL;
	int devnum;
	int i=0;
	if(retry){
		for(i=0;i<10;i++)udelay(1000);
		usb_stop();
		if (usb_init()>= 0)   {                       
			if(usb_stor_scan(0)!=0){
				printf("no system disk found\n");
				return -1;
			}
		}
	}

	devnum=oem_sys_disk_get_devnum();
	i=0;
	do{
		dev=usb_stor_get_dev(i);
		if(dev->target==devnum){                                       
			break;                                                           
		}                                                                    
		i++;                                                         
	}while(dev!=NULL);

	*index=i;
	if(dev==NULL){
		printf("failed to get device descriptor\n");
	}
	memset(dev_desc,0,sizeof(block_dev_desc_t));
	memcpy(dev_desc,dev,sizeof(block_dev_desc_t));
	return 0;
}

#elif defined(CONFIG_SYS_DISK_MMC)
#include <mmc.h>
static int oem_sys_disk_get_desc(block_dev_desc_t *dev_desc,int retry, int* index)
{
	struct mmc *mmc = find_mmc_device(SYS_DISK_CHANNEL);
	if (!mmc){
		printf("Err: Not find system disk.\n");
		return 1;
	}
	if(retry){

		if( mmc_init(mmc)) {
			printf("Err: Initiate system disk failed.\n");
			return 1;
		}
	}
	memset(dev_desc,0,sizeof(block_dev_desc_t));
	memcpy(dev_desc,&mmc->block_dev,sizeof(block_dev_desc_t));
	dev_desc->lba=mmc->sector_count;
	dev_desc->blksz=BLOCK_SIZE;
	*index=SYS_DISK_CHANNEL;
	return 0;
}
#else
static int oem_sys_disk_get_desc(block_dev_desc_t *dev_desc,int retry, int* index)
{
	return -1;
}
#endif
int oem_write_sys_disk(uint8_t *membase, int startblk, int size , int max_len)
{
	static block_dev_desc_t dev_desc;
	int index=0;

	if(0==size) return 0;

	if(NULL==membase) {
		printf("Error: invalid image buffer address %x", (int)membase);
		return 1;
	}

	if(oem_sys_disk_get_desc(&dev_desc,0,&index)) return -1;

	int cnt= ALIGN(size,dev_desc.blksz)/dev_desc.blksz;

	if(startblk+cnt>dev_desc.lba){
		printf("Error: out off range\n");
		return -1;
	}
	int n=0,trytime=1;
	int steplength=0;
	if (cnt>BURN_STEP_LENGTH*10) steplength=BURN_STEP_LENGTH;
	else steplength=BURN_STEP_LENGTH>>4;

	while(cnt){
		steplength=(steplength>cnt)? cnt:steplength;
		trytime=3;

		do{
			n = dev_desc.block_write(index, startblk,steplength, membase);
			if(n==steplength||!trytime) break;
			if(oem_sys_disk_get_desc(&dev_desc,1,&index)) return cnt;
			udelay(1000);
			printf("writen failed(%d blocks), retry it.\n",steplength);
		}while(trytime--);
		if(n!=steplength){
			printf("can not write to this disk.\n");
			return cnt;
		}else{
			printf("...");
		}
		startblk+=steplength;
		membase+=steplength<<9;
		cnt-=steplength;
		//for(i=0;i<100;i++)
		udelay(1000);
		if(steplength>=0x40)
			//oem_simpro_update(steplength<<9);
	}
	printf("Write ok.\n");
	return 0;

}

int oem_read_sys_disk(uint8_t *membase, int startblk, int size)
{
	static block_dev_desc_t dev_desc;
	int index;
	int trytime=0;

	if(oem_sys_disk_get_desc(&dev_desc,0,&index))return -1;
	
	int cnt= ALIGN(size,dev_desc.blksz)/dev_desc.blksz;

retry:
	if(((cnt+startblk )> dev_desc.lba)&&dev_desc.lba)
		printf("Error:read out of range:start block # %x, count %x device capacity  is:%x\n ",
				startblk, cnt,(unsigned int)dev_desc.lba);

	int n = dev_desc.block_read(index, startblk, cnt, (ulong *)membase);        
	flush_cache((ulong)membase, cnt * 512);/* flush cache after read */
	if(n!=cnt)  {
		if(!trytime){
			printf("%d blocks read: %s\n",n,"ERROR");
			return 1;
		}else{
			printf("Restart the disk and have another try.\n");
			trytime=1;
			if(oem_sys_disk_get_desc(&dev_desc,1,&index))return -1;													            
			goto retry;
		}
	}
	return 0;                                             
}
int init_part_table(struct part_table* locatition, struct part_table* parttable, int* tableaddr, char step)
{
	static int lasttableaddr;

	memset(parttable,0,sizeof(struct part_table)<<2);
	if(!step){
			memcpy(parttable,locatition, sizeof(struct part_table)<<2) ;
			*tableaddr=0 ;
			lasttableaddr=*tableaddr;
	}else{		
		if(step>PARTITION_NUM-3){
			printf("Error: outof partition range.\n");
			return -1;
		}
		parttable[0]= locatition[step+3];
		if(step<(PARTITION_NUM-4)){

			printf("step:%d\n",step);
			int i=1;
			parttable[1]= locatition[step+4];
			if((step+1)<(PARTITION_NUM-3))parttable[1].parttype=0x05;
			parttable[1].startLBA=0;
			for(;i<=step;i++){
					parttable[1].startLBA+=0x0010;
		      		parttable[1].startLBA+=locatition[i+3].sizeinsectors;
			}
		}
		if(step==1) *tableaddr= locatition[3].startLBA;
			else *tableaddr=lasttableaddr+ 0x000010 + locatition[step+2].sizeinsectors;
		lasttableaddr=*tableaddr;
	}	
	return 0 ;
}
int init_part_location(int sectorcount,struct part_table* locationtable)
{
	unsigned int count=0;
	unsigned int i=0;

	unsigned int sectors[PARTITION_NUM]={
		0,
		CONFIG_SYSTEM_PART_LEN<<11,
		CONFIG_UDATA_PART_LEN<<11,
		0x30*(PARTITION_NUM-3),	
		0x30+(CONFIG_CACHE_PART_LEN<<11),//make sure startLBA 0x40 aligned
		0x30+(CONFIG_MISC_PART_LEN<<11),
		0x30+(CONFIG_RAMDISK_PART_LEN<<11),
		0x30+(CONFIG_KERNEL_PART_LEN<<11)
#ifdef  CONFIG_HIBERNATE
			+(CONFIG_SYS_SDRAM<<11)
#endif
			,
		
	};
	if(sectorcount>0x500000) sectors[2]<<=1;
	//unsigned int sparesectors=SPARE_LOCATION<<11;
	for(i=4;i<PARTITION_NUM;) sectors[3]+=sectors[i++];
	for(i=1;i<4;i++) count+=sectors[i];
	printf("count:0x%x\n",count);
	sectors[0]=sectorcount-count-0x80;
	if((sectors[0]+0x10)%0x40) sectors[0]+=0x40-(sectors[0]+0x10)%0x40;
	printf("User area:0x%x\n",sectors[0]<<9);

	for(i=0;i<PARTITION_NUM;){
		locationtable[i].sizeinsectors=sectors[i];
		locationtable[i].parttype=0x83;
		locationtable[i].startCHS[0]=0x03;
		locationtable[i].startCHS[1]=0xd0;
		locationtable[i].startCHS[2]=0xFF;
		locationtable[i].endCHS[0]=0x03;
		locationtable[i].endCHS[1]=0xd0;
		locationtable[i].endCHS[2]=0xFF;
		locationtable[i].bootableflag=0;
		if(sectors[++i]==0) break;
	}
	locationtable[0].parttype=0x0C;
	locationtable[PARTITION_NUM-1].parttype=0x0C;
	if(PARTITION_NUM>4) locationtable[3].parttype=0x05;	 

	locationtable[0].startCHS[0]=0x01;
	locationtable[0].startCHS[1]=0x01;
	locationtable[0].startCHS[2]=0x00;
	locationtable[0].startLBA=0x000050;

	for(i=1;(i<PARTITION_NUM)&&(i<4);i++){
		locationtable[i].startLBA=locationtable[i-1].startLBA+locationtable[i-1].sizeinsectors;
		if(locationtable[i].startLBA%0x40){
			locationtable[i].startLBA+=0x40-locationtable[i].startLBA%0x40;
			locationtable[i].sizeinsectors-=0x40;
		}
	}

	for(;i<PARTITION_NUM;i++)
		locationtable[i].startLBA=0x000010;
	
	return 0;
}
int oem_partition(void)
{
	static block_dev_desc_t dev_desc;
	int index;
	if(oem_sys_disk_get_desc(&dev_desc,0, &index))return -1;
	lbaint_t lba=dev_desc.lba;
	printf("SYS disk:0x%x bloks(%d bytes per block)\n",(unsigned int)lba, (int)dev_desc.blksz);
	
	struct part_table partitiontable[PARTITION_NUM-2];
	struct part_table *locationtable=(struct part_table *)CONFIG_RESV_LOCTABLE;
	init_part_location(lba*(dev_desc.blksz>>9),locationtable);
	
	int tableaddr=0;
	
	char partitionnum=0;
	if(PARTITION_NUM-3) partitionnum=PARTITION_NUM-3;
		else partitionnum=1;

	unsigned int i=0;
	int imagebase[PARTITION_NUM];
	memset(imagebase,0, PARTITION_NUM*sizeof(int));
	uint8_t * buffer=(uint8_t *)CONFIG_RESV_PTBUFFER;
	memset(buffer,0,dev_desc.blksz);

	for(i=0;i<4&&i<PARTITION_NUM;i++) imagebase[i]=locationtable[i].startLBA;
	
	for(i=1;i<partitionnum;i++){
		init_part_table(locationtable,partitiontable,&tableaddr,i);
		memcpy(buffer+446 , partitiontable , sizeof(struct part_table)<<2);

		buffer[510]=0x55;
		buffer[511]=0xaa;
		oem_write_sys_disk(buffer,tableaddr,1,dev_desc.blksz);
		imagebase[i+3]=tableaddr+0x10;
	}

	int *p=(int*)buffer;
	memcpy(p + CONFIG_IMAGE_BASE_BUFFER + 1 , imagebase , PARTITION_NUM*sizeof(int));
	int ret=tableaddr;

	init_part_table(locationtable,partitiontable,&tableaddr,0);
	memcpy(buffer+446 , partitiontable , sizeof(struct part_table)<<2);

	buffer[510]=0x55;
	buffer[511]=0xaa;

	buffer[CONFIG_PART_STATE_BASE]=DISK_PARTITIONED;//mark the disk has been partitioned

	oem_write_sys_disk(buffer,tableaddr,1,(int)dev_desc.blksz);
	printf("PARTITION_NUM :%d\n   image start:%x\n\n", PARTITION_NUM ,p[CONFIG_IMAGE_BASE_BUFFER\
			+CONFIG_SYS_RAMDISK_PART_NUM]);
	return ret;

}
int oem_get_base(int partnum)
{
	int startblk=0;
	int *buffer=(int *)CONFIG_RESV_PTBUFFER;
	memset((char *)buffer,0,512);
	if(oem_read_sys_disk((uint8_t *) buffer , 0 ,1)) return 0;

	buffer+=CONFIG_IMAGE_BASE_BUFFER;
	startblk=buffer[partnum];
	printf("startblk:%x\n",startblk);

	return startblk;
}
int oem_check_part_state(void)
{
	int *buffer=(int *)CONFIG_RESV_PTBUFFER;
	memset((char *)buffer,0,512);
	if(oem_read_sys_disk((uint8_t *) buffer , 0 ,1)){ 
		printf("failed to got disk partition state.\n");
		return 0;
	}

	return (DISK_PARTITIONED==buffer[CONFIG_PART_STATE_BASE]);

}

int oem_get_system_length(void)
{
	int *buffer=(int *)CONFIG_RESV_PTBUFFER;               
	memset((char *)buffer,0,512);                          
	if(oem_read_sys_disk((uint8_t *)buffer , 0 ,1)) return 0;

	return  buffer[CONFIG_SYSTEM_REAL_LEN];
}
int oem_set_system_length(int length)
{
	uint8_t *buffer=(uint8_t *)CONFIG_RESV_PTBUFFER;               
	memset((char *)buffer,0,512);                          
	if(oem_read_sys_disk((char *) buffer , 0 , 512)) return -1;

	buffer[CONFIG_SYSTEM_REAL_LEN]=length;

	if(oem_write_sys_disk(buffer , 0 , 512 , 512)) return -1;

	return 0;

}
int oem_clear_env(void)
{
	uint8_t *buffer=(uint8_t *)CONFIG_RESV_PTBUFFER;
	memset((char *)buffer,0,CONFIG_ENV_SIZE);
	oem_write_sys_disk((uint8_t *) buffer , CONFIG_ENV_OFFSET , CONFIG_ENV_SIZE , CONFIG_ENV_SIZE);
	return 0;
}

#ifdef CONFIG_HIBERNATE
int oem_get_hibernatebase(void)
{
	return  CONFIG_SYS_DISK_KERNEL_OFFS+(CONFIG_KERNEL_PART_LEN<<11);
}
#endif
